/***************************************************************************
创建者：彭秋明
 开始时间：         : 2017.03.01
 copyright            : (C) 同川科技
 修改说明：（每次有修改就添加一条，带有 修改人，修改时间，修改描述）

 ***************************************************************************
 *   定义各种求解器，包括Pos、Vel、Acc的正逆解
 *
*                                                                         *
 ***************************************************************************/

#include "solverTest.hpp"
#include <kdl/frames_io.hpp>
#include <kdl/framevel_io.hpp>
#include <kdl/frameacc_io.hpp>
#include <kdl/kinfam_io.hpp>
#include <iostream>
//#include <sys/time.h>
//#include <time.h>

#define IF_DEBUG false
#define TEST_NUM 10000

using namespace KDL;

SolverTest::SolverTest()
{
    this->puma = Puma560();
    this->jnPuma = this->puma.getNrOfJoints();
    this->jointPuma.resize(this->jnPuma);
}

SolverTest::~SolverTest()
{

}

void SolverTest::writeSolverTestTime(std::string str, MeasureTime *timeTest)
{
    //outfile.open("./solverTestTime.txt", std::ios::app); //std::ios::app表示在文件末尾追加
    outfile.open("./TestTime.txt", std::ios::app);
    //检查文件是否正常打开
    if(!outfile)
    {
       //std::cout <<"solverTestTime.txt can't open" << std::endl;
        std::cout <<"TestTime.txt can't open" << std::endl;
       //打开失败，结束程序
       abort();
    }
    else
    {
        outfile << str ;
        outfile << "minTime=" << timeTest->minTime / 1000 << "us;" << "\t";
        outfile << "maxTime=" << timeTest->maxTime / 1000 << "us;" << "\t";
        outfile << "averageTime=" << timeTest->averageTime / 1000 << "us;" << "\t";
        outfile << "allTime=" << timeTest->addTime / 1000 << "us" << "\n" << std::endl;
        outfile.close();
    }
    timeTest->reset();
}

void SolverTest::chainIkSolverPos(const JntArray &joint, ChainFkSolverPos &fkSolverPos, JntArray &jointInit,
                                  ChainIkSolverPos &ikSolverPos, JntArray &jointOut, std::string str)
{
    int jn = joint.rows();
    Frame fp,fk;
    MeasureTime* timeTest = new MeasureTime("IkSolverPos时间", 1, 1, TEST_NUM);
    if (IF_DEBUG)
    {
        std::cout << "jointArray:" << std::endl;
        for (int i = 0; i < jn; i++)
        {
            std::cout << joint(i) << " ";
        }
        std::cout << std::endl;
    }
    assert(fkSolverPos.JntToCart(joint, fp) == 0);
    if (IF_DEBUG)
    {
        std::cout << "frame of jointArray:" << std::endl;
        std::cout << fp << std::endl;
    }
    assert(ikSolverPos.CartToJnt(jointInit, fp, jointOut) == 0);
    if(IF_DEBUG)
    {
        std::cout<<"inverse soulution:"<<std::endl;
        for(int i = 0; i < jn; i++)
        {
            std::cout << jointOut(i) << " ";
        }
        std::cout << std::endl;
    }
    assert(fkSolverPos.JntToCart(jointOut, fk) == 0);
    if(IF_DEBUG)
    {
        std::cout<< "frame of inverse output:" << std::endl;
        std::cout << fk << std::endl;
    }
//    assert(fk==fp);
    //test the time of IkSolverPos
    for(int j = 0; j < TEST_NUM; j++)
    {
        timeTest->recordInTime();
        assert(ikSolverPos.CartToJnt(jointInit, fp, jointOut) == 0);
//        assert(this->jointPuma==jointPumaOut);
        timeTest->recordOutTime();
     }
    writeSolverTestTime(str, timeTest);
    delete timeTest;
    timeTest = NULL;
}

void SolverTest::chainSolverVel(const Chain &chain, ChainFkSolverVel &fkSolverVel, ChainIkSolverVel &ikSolverVel, std::string str)
{
    int jn = chain.getNrOfJoints();
    JntArray q(jn);
    JntArray qdot(jn);
    MeasureTime* timeTest1 = new MeasureTime("FkSolverVel时间", 1, 1, TEST_NUM);
    MeasureTime* timeTest2 = new MeasureTime("IkSolverVel时间", 1, 1, TEST_NUM);
    for(int i = 0; i < jn; i++)
    {
        random(q(i));
        random(qdot(i));
    }
    JntArrayVel qvel(q, qdot);
    JntArray qdot_solved(jn);
    FrameVel cart;
    assert(fkSolverVel.JntToCart(qvel, cart) == 0);
    std::cout << "forward:" << std::endl;
    if(IF_DEBUG)
    {
        std::cout << "forward solution of velocity:" << std::endl;
        std::cout << "dot of joint:" << std::endl;
        for(int i = 0; i < jn; i++)
        {
            std::cout << qvel.qdot(i) << " ";
        }
        std::cout << std::endl;
        std::cout << "frame of velocity:" << std::endl;
        std::cout << cart << std::endl;
    }
    std::string str1 = "正解Vel_recursive测试时间：";
    //test the time of FkSolverVel
    for(int i = 0; i < TEST_NUM; i++)
    {
        timeTest1->recordInTime();
        assert(fkSolverVel.JntToCart(qvel, cart) == 0);
        timeTest1->recordOutTime();
    }
    writeSolverTestTime(str1, timeTest1);
    delete timeTest1;
    timeTest1 = NULL;

    assert(ikSolverVel.CartToJnt(qvel.q, cart.deriv(), qdot_solved) >= 0);
    qvel.qdot = qdot_solved;
    FrameVel cart_solved;
    assert(fkSolverVel.JntToCart(qvel, cart_solved) == 0);
    assert(Equal(cart.deriv(), cart_solved.deriv(), 1e-5));
    std::cout << "inverse:" << std::endl;
    if(IF_DEBUG)
    {
        std::cout << "inverse solution of velocity:" << std::endl;
        std::cout << "dot of joint:" << std::endl;
        for(int i = 0; i < jn; i++)
        {
            std::cout << qdot_solved(i) << " ";
        }
        std::cout << std::endl;
        std::cout << "frame of velocity:" << std::endl;
        std::cout << cart_solved << std::endl;
    }
    //test the time of IkSolverVel
    for(int i = 0; i < TEST_NUM; i++)
    {
        timeTest2->recordInTime();
        assert(ikSolverVel.CartToJnt(qvel.q, cart.deriv(), qdot_solved) >= 0);
        timeTest2->recordOutTime();
    }
    writeSolverTestTime(str, timeTest2);
    delete timeTest2;
    timeTest2 = NULL;
}

//void SolverTest::chainSolverAcc(const Chain &chain, ChainFkSolverAcc &fkSolverAcc, ChainIkSolverAcc &ikSolverAcc)
//{
//    int jn = chain.getNrOfJoints();
//    JntArray q(jn);
//    JntArray qdot(jn);
//    JntArray qdotdot(jn);
//    MeasureTime* timeTest1 = new MeasureTime("FkSolverAcc时间", 1, 1, TEST_NUM);
//    MeasureTime* timeTest2 = new MeasureTime("IkSolverAcc时间", 1, 1, TEST_NUM);
//    for(int i = 0; i < jn; i++)
//    {
//        random(q(i));
//        random(qdot(i));
//        random(qdotdot(i));
//    }
//    JntArrayAcc qacc(q, qdot, qdotdot);
////    JntArray qdotdot_solved(jn);
//     JntArrayAcc qdotdot_solved(jn);
//    FrameAcc cart;
//    assert(fkSolverAcc.JntToCart(qacc, cart) == 0);
//    if(IF_DEBUG)
//    {
//        std::cout << "forward solution of acc:" << std::endl;
//        std::cout << "dot of joint:" << std::endl;
//        for(int i = 0; i < jn; i++)
//        {
//            std::cout << qacc.qdotdot(i) << " ";
//        }
//        std::cout << std::endl;
//        std::cout << "frame of acc:" << std::endl;
//        std::cout << cart << std::endl;
//    }
//    //test the time of FkSolverAcc
//    for(int i = 0; i < TEST_NUM; i++)
//    {
//        timeTest1->recordInTime();
//        assert(fkSolverAcc.JntToCart(qacc, cart) == 0);
//        timeTest1->recordOutTime();
//    }
//    delete timeTest1;
//    timeTest1 = NULL;

//    assert(ikSolverAcc.CartToJnt(qacc.q, cart, qdotdot_solved) >= 0);
//    qacc.qdotdot = qdotdot_solved;
//    FrameAcc cart_solved;
//    assert(fkSolverVel.JntToCart(qacc, cart_solved) == 0);
//    assert(Equal(cart, cart_solved, 1e-5));
//    std::cout << "inverse:" << std::endl;
//    if(IF_DEBUG)
//    {
//        std::cout << "inverse solution of acc:" << std::endl;
//        std::cout << "dot of joint:" << std::endl;
//        for(int i = 0; i < jn; i++)
//        {
//            std::cout << qdotdot_solved(i) << " ";
//        }
//        std::cout << std::endl;
//        std::cout << "frame of acc:" << std::endl;
//        std::cout << cart_solved << std::endl;
//    }
//    //test the time of IkSolverAcc
//    for(int i = 0; i < TEST_NUM; i++)
//    {
//        timeTest2->recordInTime();
//        assert(ikSolverAcc.CartToJnt(qacc.q, cart, qdotdot_solved) >= 0);
//        timeTest2->recordOutTime();
//    }
//    delete timeTest2;
//    timeTest2 = NULL;
//}

void SolverTest::chainFkSolverPosTest()
{
    ChainFkSolverPos_recursive pumaFkSolverPos = ChainFkSolverPos_recursive(this->puma);
    Frame fp, fp1;
    MeasureTime* timeTest = new MeasureTime("FkSolverPos时间", 1, 1, TEST_NUM);

/********model--puma********/
     for(int i = 0; i < this->jnPuma; i++)
     {
         random(this->jointPuma(i));
      }
    assert(pumaFkSolverPos.JntToCart(this->jointPuma, fp) == 0);
    if(IF_DEBUG)
    {
        std::cout << "forword soulution of Puma560:" << std::endl;
        std::cout << fp << std::endl;
    }
    str = "正解Pos_recursive测试时间：";
    //test the time of FkSolverPos
    for(int j=0; j<TEST_NUM; j++)
    {
        timeTest->recordInTime();
       fp1 = fp;
       assert(pumaFkSolverPos.JntToCart(this->jointPuma, fp) == 0);
       assert(fp == fp1);
       timeTest->recordOutTime();
    }
    writeSolverTestTime(str, timeTest);
    delete timeTest;
    timeTest = NULL;
}

void SolverTest::chainIkSolverPosTest_LMA()
{
    JntArray jointPumaInit = JntArray(this->jnPuma);
    JntArray jointPumaOut = JntArray(this->jnPuma);
    ChainFkSolverPos_recursive pumaFkSolverPos = ChainFkSolverPos_recursive(this->puma);
    ChainIkSolverPos_LMA pumaIkSolverPos_LMA = ChainIkSolverPos_LMA(this->puma);

//********model--puma********//
    for(int i = 0; i < jnPuma; i++)
    {
        random(this->jointPuma(i));
    }
    std::cout << "inverse solution of Puma560 (LMA) :" << std::endl;
    str = "逆解Pos_LMA测试时间：";
    this->chainIkSolverPos(this->jointPuma, pumaFkSolverPos, jointPumaInit, pumaIkSolverPos_LMA, jointPumaOut, str);
}

void SolverTest::chainIkSolverPosTest_NR()
{
    JntArray jointPumaInit = JntArray(this->jnPuma);
    JntArray jointPumaOut = JntArray(this->jnPuma);
    ChainFkSolverPos_recursive pumaFkSolverPos = ChainFkSolverPos_recursive(this->puma);
    ChainIkSolverVel_pinv pumaIkSolverVel = ChainIkSolverVel_pinv(this->puma);
    ChainIkSolverPos_NR pumaIkSolverPos_NR = ChainIkSolverPos_NR(this->puma, pumaFkSolverPos, pumaIkSolverVel);
//    ChainIkSolverVel_pinv_givens pumaIkSolverVel_givens =ChainIkSolverVel_pinv_givens(this->puma);
//    ChainIkSolverPos_NR pumaIkSolverPos_NR_givens = ChainIkSolverPos_NR(this->puma, pumaFkSolverPos, pumaIkSolverVel_givens,1000);

    //********NR********//

    //********model-Puma360********//
        for(int i = 0; i < this->jnPuma; i++)
        {
            random(this->jointPuma(i));
        }
        std::cout<< "inverse soulution of Puma560 (NR) :" << std::endl;
        str = "逆解Pos_NR测试时间：";
        this->chainIkSolverPos(this->jointPuma, pumaFkSolverPos, jointPumaInit, pumaIkSolverPos_NR, jointPumaOut, str);

////********NR_givens********//

////********model-Puma360********//
//        for(int i = 0; i < this->jnPuma; i++)
//        {
//            random(this->jointPuma(i));
//        }
//        double tmp;
//        for(int i = 0; i < this->jnPuma; i++)
//        {
//            random(tmp);
//            jointPumaInit(i) = this->jointPuma(i) + 0.1*tmp;
//        }
//        std::cout<< "inverse soulution of Puma560 (NR_givens) :" << std::endl;
//        this->chainIkSolverPos(this->jointPuma, pumaFkSolverPos, jointPumaInit, pumaIkSolverPos_NR_givens, jointPumaOut);
}

void SolverTest::chainIkSolverPosTest_NR_JL()
{
    JntArray jointPumaInit(this->jnPuma);
    JntArray jointPumaOut(this->jnPuma);
    ChainFkSolverPos_recursive pumaFkSolverPos = ChainFkSolverPos_recursive(this->puma);
    ChainIkSolverVel_pinv pumaIkSolverVel = ChainIkSolverVel_pinv(this->puma);
    JntArray jointMin(this->jnPuma);
    JntArray jointMax(this->jnPuma);
    for(int i = 0; i < this->jnPuma; i++)
    {
        jointMin(i) = -2*M_PI;
        jointMax(i) = 2*M_PI;
    }
    ChainIkSolverPos_NR_JL pumaIkSolverPos_NR_JL = ChainIkSolverPos_NR_JL(this->puma, jointMin, jointMax, pumaFkSolverPos, pumaIkSolverVel);
    for(int i = 0; i < this->jnPuma; i++)
    {
        random(this->jointPuma(i));
    }
    std::cout<< "inverse soulution of Puma560 (NR_JL) :" << std::endl;
    str = "逆解Pos_NR_JL测试时间：";
    this->chainIkSolverPos(this->jointPuma, pumaFkSolverPos, jointPumaInit, pumaIkSolverPos_NR_JL, jointPumaOut, str);
}

void SolverTest::chainFkVelAndIkVelTest()
{
    ChainFkSolverVel_recursive pumaFkSolverVel(this->puma);
    ChainIkSolverVel_pinv pumaIkSolverVel_pinv(this->puma);
    ChainIkSolverVel_pinv_givens pumaIkSolverVel_pinv_givens(this->puma);
    ChainIkSolverVel_wdls pumaIkSolverVel_wdls(this->puma);
    std::cout << "pinv of Puma360:" << std::endl;
    str = "逆解Vel_PINV测试时间：";
    this->chainSolverVel(this->puma, pumaFkSolverVel, pumaIkSolverVel_pinv, str);
    std::cout << "pinv_givens of Puma360:" << std::endl;
    str = "逆解Vel_PINV_GIVENS测试时间：";
    this->chainSolverVel(this->puma, pumaFkSolverVel, pumaIkSolverVel_pinv_givens, str);
    std::cout << "wdls of Puma360:" << std::endl;
    str = "逆解Vel_WDLS测试时间：";
    this->chainSolverVel(this->puma, pumaFkSolverVel, pumaIkSolverVel_wdls, str);
}

void SolverTest::chainIkVelTest_NSO()
{
    ChainFkSolverVel_recursive pumaFkSolverVel(this->puma);
    ChainIkSolverVel_pinv_nso pumaIkSolverVel_pinv_nso(this->puma);
    JntArray q(this->jnPuma);
    JntArray qdot(this->jnPuma);
    MeasureTime* timeTest1 = new MeasureTime("IkSolverVel_NSO时间", 1, 1, TEST_NUM);
    for(int i = 0; i < this->jnPuma; i++)
    {
        random(q(i));
        random(qdot(i));
    }
    JntArrayVel qvel(q, qdot);
    JntArray qdot_solved(this->jnPuma);
    FrameVel cart;
    assert(pumaFkSolverVel.JntToCart(qvel, cart)==0);
    std::cout << "pinv_nso of Puma560:" <<std::endl;
    std::cout << pumaIkSolverVel_pinv_nso.CartToJnt(q, cart.deriv(), qdot_solved) <<std::endl;
    assert(pumaIkSolverVel_pinv_nso.CartToJnt(q, cart.deriv(), qdot_solved)==0);

    for(int i = 0; i < TEST_NUM; i++)
    {
        timeTest1->recordInTime();
        assert(pumaIkSolverVel_pinv_nso.CartToJnt(q, cart.deriv(), qdot_solved)==0);
        timeTest1->recordOutTime();
    }
    delete timeTest1;
    timeTest1 = NULL;
}

void SolverTest::chainFkAccAndIkAccTest()
{
//    ChainFkSolverAcc pumaFkSolverAcc(this->puma); //ChainFkSolverAcc没有构造函数
//    ChainIkSolverAcc pumaIkSolverAcc(this->puma);
//    std::cout << "acc solver of Puma360:" << std::endl;
//    this->chainSolverAcc(this->puma, pumaFkSolverAcc, pumaIkSolverAcc);
}

void SolverTest::chainIdSolverTest_RNE()
{
    int jn = this->puma.getNrOfJoints();
    JntArray q(jn);
    JntArray qdot(jn);
    JntArray qdotdot(jn);
    MeasureTime* timeTest = new MeasureTime("IdSolver RNE", 1, 1, TEST_NUM);
    for(int i = 0; i < jn; i++)
    {
        random(q(i));
        random(qdot(i));
        random(qdotdot(i));
    }
    JntArray tor(jn);
    ChainIdSolver_RNE idSolver_RNE(this->puma, Vector(0.0, 0.0, -9.81));
    Wrenches f(this->puma.getNrOfSegments());
    std::cout << "idsolver RNE:" << std::endl;
    assert(idSolver_RNE.CartToJnt(q, qdot, qdotdot, f, tor) == 0);
    if(IF_DEBUG)
    {
        std::cout << "torque of RNE:" << std::endl;
        std::cout << tor << std::endl;
    }
    str = "Idsolver_RNE测试时间：";
    for(int i = 0; i < TEST_NUM; i++)
    {
        timeTest->recordInTime();
        assert(idSolver_RNE.CartToJnt(q, qdot, qdotdot, f, tor) == 0);
        timeTest->recordOutTime();
    }
    writeSolverTestTime(str, timeTest);
    delete timeTest;
    timeTest = NULL;
}

//void SolverTest::chainIdSolverTest_Vereshchagin()
//{
//    Vector constrainXLinear(1.0, 0.0, 0.0);
//    Vector constrainXAngular(0.0, 0.0, 0.0);
//    //Vector constrainYLinear(0.0, 0.0, 0.0);
//    //Vector constrainYAngular(0.0, 0.0, 0.0);
//    Twist constrainForcesX(constrainXLinear, constrainXAngular);
//    //Twist constrainForcesY(constrainYLinear, constrainYAngular);

//    Jacobian alpha(1);
//    alpha.setColumn(0, constrainForcesX);

//    //Acceleration energy at the end-effector
//    JntArray betha(1);
//    betha(0) = 0.0;

//    //arm root acceleration
//    Vector linearAcc(0.0, 10, 0.0);
//    Vector angularAcc(0.0, 0.0, 0.0);
//    Twist twist(linearAcc, angularAcc);

//    //external forces on the arm
//    Vector externalForce1(0.0, 0.0, 0.0);
//    Vector externalTorque1(0.0, 0.0, 0.0);
//    Vector externalForce2(0.0, 0.0, 0.0);
//    Vector externalTorque2(0.0, 0.0, 0.0);
//    Wrench externalNetForce1(externalForce1, externalTorque1);
//    Wrench externalNetForce2(externalForce2, externalTorque2);
//    Wrenches externalNetForce;
//    externalNetForce.push_back(externalNetForce1);
//    externalNetForce.push_back(externalNetForce2);

//    int jn = this->puma.getNrOfJoints();
//    JntArray q(jn);
//    JntArray qdot(jn);
//    JntArray qdotdot(jn);
//    MeasureTime* timeTest = new MeasureTime("IdSolver_Vereshchagin", 1, 1, TEST_NUM);
//    for(int i = 0; i < jn; i++)
//    {
//        random(q(i));
//        random(qdot(i));
//        random(qdotdot(i));
//    }
//    JntArray tor(jn);

//    //MeasureTime* timeTest = new MeasureTime("IdSolver_Vereshchagin", 1, 1, TEST_NUM);
//    int numerOfConstraints = 1;
//    ChainIdSolver_Vereshchagin idSolver_Vereshchagin(this->puma, twist, numerOfConstraints);

//    Wrenches f(this->puma.getNrOfSegments());
//    const int k = 1;
//    JntArray jointPoses[k];
//    JntArray jointRates[k];
//    JntArray jointAcc[k];
//    JntArray jointTorques[k];
//    for (int i = 0; i < k; i++)
//    {
//        JntArray jointValues(this->puma.getNrOfJoints());
//        jointPoses[i] = jointValues;
//        jointRates[i] = jointValues;
//        jointAcc[i] = jointValues;
//        jointTorques[i] = jointValues;
//    }
//    JntArray jointInitPose(this->puma.getNrOfJoints());
//    jointInitPose(0) = 0.0;
//    jointInitPose(1) = M_PI / 6.0;
//    jointPoses[0](0) = jointInitPose(0);
//    jointPoses[0](1) = jointInitPose(1);

//    std::cout << "IdSolver_Vereshchagin:" << std::endl;
//    //assert(Equal(SolverI::E_NOERROR, idSolver_Vereshchagin.CartToJnt(q, qdot, qdotdot, alpha, betha, externalNetForce, tor) == 0));
//    std::cout << idSolver_Vereshchagin.CartToJnt(q, qdot, qdotdot, alpha, betha, externalNetForce, tor) << std::endl;
//    assert(idSolver_Vereshchagin.CartToJnt(jointPoses[0], jointRates[0], jointAcc[0], alpha, betha, externalNetForce, jointTorques[0]) == 0);
////    if(IF_DEBUG)
////    {
////        std::cout << "torque of Vereshchagin:" << std::endl;
////        std::cout << tor << std::endl;
////    }
//    for(int i = 0; i < TEST_NUM; i++)
//    {
//        timeTest->recordInTime();
//        //assert(Equal(SolverI::E_NOERROR, idSolver_Vereshchagin.CartToJnt(q, qdot, qdotdot, alpha, betha, externalNetForce, tor) == 0));
//        assert(idSolver_Vereshchagin.CartToJnt(jointPoses[0], jointRates[0], jointAcc[0], alpha, betha, externalNetForce, jointTorques[0]) == 0);
//        timeTest->recordOutTime();
//    }
//    delete timeTest;
//    timeTest = NULL;
//}
